/*
 * ControlLoop.cpp
 *
 *  Created on: Aug 6, 2009
 *      Author: naveenoid
 */

#include "ControlLoop.hpp"
#include <ecl/time.hpp>
#include <stdio.h>
#include "./../Common/Common.hpp"

using Octopus::Common::wait;
using Octopus::Common::uWait;

using Octopus::BoardSetup::TitechBoard;
using ECL::Time::StopWatch;
using ECL::Time::TimeStamp;
using Octopus::ToolKit::Log::Logger;
//using ECL::Time::

namespace Octopus {

namespace MainLoop {

//ControlLoop * ControlLoop::controlLoop;


Loop * ControlLoop::getInstance(Logger *log) {
	if(loop == NULL)
	{
		loop = new ControlLoop(log);
	}
	return(loop);
}

void ControlLoop::remove() {
	printf("Removing Control Loop\n");

	while(!loopFinished)
	{
//		uWait(10000);
		wait(0.1);
		printf("waiting for thread to finish\n");
	}

	delete(this);
}

void ControlLoop::start() {

	int ctr = 1;
	StopWatch stopwatch;
	TimeStamp time;

	long int loopTime = (long int)LOOP_TIME;
	long int time1 = 0, time2 = 0, time3 = 0, timeDiff = 0, totTime = 0, time1Prev = 0;
	double avgTime;

	int lMeasDigital=0, motorOutDigital= 0;
	int pulseOut = 0;
	double fMeasAnalog = 0.0, motorOutAnalog = 0.0, error = 0.0, TsetPoint = 0.0, lastError = 0.0, errorDot = 0.0;
	double errorInt = 0.0;
	double offset = 0.0;

	loopFinished = false;
	//Initialising Loop

	printf("Starting Pretension Initialisation\n\n");
	while(fMeasAnalog<=0.0)
	{
		mutex.lock();
		if(!continueLoop)
		{
			printf("Loop trying to stop\n");
			mutex.unlock();
			break;
		}
		mutex.unlock();

		time = stopwatch.elapsed();
		time1 = time.usec();
//		printf("Initial = %ld \t",time1);
//		wait(0.10);

//		uWait(50000);
// Task

//		vector<int> temp(MAX_NUM_OF_SENSOR,0);
		vector<int> temp = titechBoard->readAD();
//		printf("getting each sensor from vector  ");
		for(int i=0; i<MAX_NUM_OF_SENSOR && i< (int)temp.size();i++)
//		for(int i=0; i<temp.size();i++)
		{
			lMeasDigital = temp.at(i);
			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * ((1023 - (double)lMeasDigital)/1023);

			pulseOut = (int)(1100+ 800*((double)offset/1260));
//			double tempPulseOut = (((double)pulseOut - 1100)*((3.5*360)/800));

			printf("Sensor : %d, Actual(N) : %f,  Offset : %f", lMeasDigital, fMeasAnalog, offset);
			titechBoard->writeMotor(i,pulseOut);

//			printf("Sensor %d : %d ", i+1, temp.at(i+1));
		}
		printf("\n");

		if(offset<=100.0)
		{
			offset+=10.0;
		}
		else if(offset >= 100.0 && offset <= 105.0)
		{
			offset+=0.5;
		}
		else if(offset>105.0)
		{
			offset+=0.01;
		}
		time = stopwatch.elapsed();
		time2 = time.usec();
//		printf("Final = %ld \t",time2);

		timeDiff = loopTime-(time2-time1);
//		printf("Must wait for %ld uSec\n", timeDiff);

		uWait(timeDiff);
		stopwatch.restart();

	}

	printf("\n\nFinished Pretension Initialisation, Starting Main Loop\n\n");


	//Main LOOP
	while(true)
	{
		mutex.lock();
		if(!continueLoop)
		{
			printf("Loop trying to stop\n");
			mutex.unlock();
			break;
		}
		mutex.unlock();

		time = stopwatch.elapsed();
		time1 = time.usec();
//		printf("Initial = %ld \t",time1);
//		wait(0.10);

//		uWait(50000);
// Task

//		vector<int> temp(MAX_NUM_OF_SENSOR,0);
		vector<int> temp = titechBoard->readAD();
//		printf("getting each sensor from vector  ");
		for(int i=0; i<MAX_NUM_OF_SENSOR && i< (int)temp.size();i++)
//		for(int i=0; i<temp.size();i++)
		{
			lMeasDigital = temp.at(i);
			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * ((1023 - (double)lMeasDigital)/1023);

			setPointMutex.lock();
			TsetPoint = setPoints.at(i);
			setPointMutex.unlock();

			lastError = error;
			error = TsetPoint - fMeasAnalog;
			errorInt += (error * loopTime) / 1000000;
			errorDot = 1000000 * ((error-lastError) / loopTime);

			motorOutAnalog = (TsetPoint * 360)/(PI * WHEEL_DIAMETER* (POT_SPRING_STIFFNESS + LOAD_SPRING_STIFFNESS)) + (double)K_P * error + (double)K_D * errorDot + (double)K_I * errorInt;

			motorOutDigital = 1100 + ((((int)motorOutAnalog + offset)*800)/(3.5*360));
			if(motorOutDigital<1100) {

				motorOutDigital = 1100;
			}

//			pulseOut = (int)(1100 + 800*((double)lMeasDigital/1023));

//			double thetaTemp = ((double)theta/1023);
//			pulseOut = (int)(1100 + 800*((double)heta/1023));
//			pulseOut = (int)(1100+ 800*(offset/1260));


//			double tempPulseOut = (((double)pulseOut - 1100)*((3.5*360)/800));

			printf("SetPoint %f, Actual(N) %f, Error : %f, ErrorDot : %f, ErrorInteg : %f, KPe : %f, KDeDot : %f, KIIntE : %f", TsetPoint, fMeasAnalog, error, errorDot, errorInt, (double)K_P * error, (double)K_D * errorDot, (double)K_I * errorInt);
//			titechBoard->writeMotor(i,pulseOut);
			titechBoard->writeMotor(i,motorOutDigital);

//			printf("Sensor %d : %d ", i+1, temp.at(i+1));
		}
		printf("\n");



		time = stopwatch.elapsed();
		time2 = time.usec();
//		printf("Final = %ld \t",time2);

		timeDiff = loopTime-(time2-time1);
//		printf("Must wait for %ld uSec\n", timeDiff);

		uWait(timeDiff);

		time = stopwatch.elapsed();

		time3 = time.usec();

		// Job
		totTime += time3 - time1;
		avgTime = (double)totTime / (double)ctr;
		ctr++;

//		printf ("Time3 is %ld uSec and total loopTime is %ld uSec and Average LoopTime is %f\n", time3, totTime, avgTime);

		time1Prev = time1;

		stopwatch.restart();

	}

	//Resetting motors

	for(int i=0; i<MAX_NUM_OF_MOTOR ;i++)
	//		for(int i=0; i<temp.size();i++)
	{
		titechBoard->writeMotor(i,1100);
	}


	loopFinished = true;
	printf ("Average LoopTime was %f\n", avgTime);
	printf("ControlLoop thread closing\n");
}

bool ControlLoop::stop()
{
	printf("Trying to stop Loop");
	mutex.lock();
	continueLoop = false;
	mutex.unlock();
	return(continueLoop);
}
bool ControlLoop::finished()
{
	return(loopFinished);
}

bool ControlLoop::changeSetPoints(vector<double> sp) {
	setPointMutex.lock();
	setPoints = sp;
	setPointMutex.unlock();
	return(true);
}

ControlLoop::ControlLoop(Logger *log) : sensorValues(MAX_NUM_OF_SENSOR,0), motorCommands(MAX_NUM_OF_MOTOR,0), setPoints(MAX_NUM_OF_MOTOR,0.0) {
	logger = log;
	titechBoard = TitechBoard::getInstance();
	continueLoop = true;
	loopFinished = true;
	offsetForce = 0.0;
	printf("Created New ControlLoop\n");
}

ControlLoop::~ControlLoop() {
	titechBoard->remove();
}

}

}
